/**
 * @author
 * Aldebaran Robotics (c) 2007 All Rights Reserved - This is an example of use.\n
 *
 */
#include <iostream>
#include "gvmsample.h"
#include "alproxy.h"

// For resolution and colorspace definitions + utility functions.
#include "alvisiondefinitions.h"
#include "alimage.h"

#include "opencv/highgui.h"

using namespace std;

namespace AL
{

const std::string GVMsample::frameMemoryAddress = "naoVisionCurrentFrame";
//______________________________________________
// constructor
//______________________________________________
GVMsample::GVMsample( ALPtr<ALBroker> pBroker, const std::string& pName ):
  ALModule(pBroker , pName),
  fIplImageHeader(NULL),
  fRegisteredToVim(false) {

  // Describe the module here.
  setModuleDescription( "This is an example of a generic video module. "
    "It can be used to save images returned by the camera. "
    "It internally uses OpenCv's cvSaveImage function. "
    "Refer to the Example codes section below for an example of use.");

  // Define the module example.
  addModuleExample( "Python",
                     "  # Create a proxy to the module \n"
                     "  sampleProxy = ALProxy('GVMsample', '127.0.0.1', 9559)\n\n"
                     "  # Register our module to the Video Input Module. \n"
                     "  sampleProxy.registerToVIM(1, 13)\n\n"
                     "  # Save image in remote mode. \n"
                     "  sampleProxy.saveImageRemote('/img', 'jpg') \n\n"
                     "  # Unregister.\n"
                     "  sampleProxy.unRegisterFromVIM()\n"
                  );

  // Define bound methods with their description.
  functionName( "registerToVIM", getName(), "Register to the V.I.M." );
  addParam("pResolution", "Resolution requested.");
  addParam("pColorSpace", "Colorspace requested.");
  addParam("pFps", "Fps (frames per second) requested."); 
  BIND_METHOD( GVMsample::registerToVIM );
  
  functionName( "unRegisterFromVIM", getName(), "Unregister from the V.I.M." );
  BIND_METHOD( GVMsample::unRegisterFromVIM );
  
  functionName( "saveImageLocal", getName(), "Save an image received from the camera." );
  addParam( "pName", "name and path of the picture (without format extension)" );
  addParam( "pImageFormat", "format extension such as bmp, jpg, tiff, etc.)" );
  BIND_METHOD( GVMsample::saveImageLocal );

  functionName( "saveImageRemote", getName(), "Save an image received from the camera. to be used if the genericvideomodule is a remote module." );
  addParam( "pName", "name and path of the picture (without format extension)" );
  addParam( "pImageFormat", "format extension such as bmp, jpg, tiff, etc.)" );
  BIND_METHOD( GVMsample::saveImageRemote );

  functionName( "startGrabbing", getName(), "Starts a grabber thread that runs at the specified framerate and dumps each frame into ALMemory.");
  BIND_METHOD( GVMsample::startGrabbing );

  functionName( "stopGrabbing", getName(), "Stops the frame grabber thread.");
  BIND_METHOD( GVMsample::stopGrabbing );

  functionName( "grabFrames", getName(), "Actual worker. DON'T CALL!!! Should find a way to pcall it without exporting through broker!!!");
  BIND_METHOD( GVMsample::grabFrames );
}


void GVMsample::init() {

  // Create a proxy to the logger module.
  // If this fails, we throw an exception and the module will not be registered.
  try {
    fLogProxy = getParentBroker()->getLoggerProxy();  
  } catch (const ALError& e) {
    throw ALError(getName(), "GVMsample()", "Fail to create a proxy to ALLogger "
      "module. Msg " + e.toString() + "\nModule will abort.");
  }

  // Create a proxy to the video input module.
  try {
    fCamProxy = getParentBroker()->getProxy( "ALVideoDevice" );
  } catch (const ALError& e) {
    throw ALError(getName(), "GVMsample()", "Could not create a proxy to ALVideoDevice " +
      e.toString());
  }

  //Create a proxy for the memory module
  try {
	fMemoryProxy = getParentBroker()->getProxy ( "ALMemory" );
  } catch (const ALError& e){
	throw ALError(getName(), "GVMsample()", "Could not create a proxy to ALMemory " +
			e.toString());
  }


  //Create a proxy to this module (needed if we want to make a pcall)
  try {
	fThisProxy = getProxy( "GVMsample" );
  } catch (const ALError& e)
  {
	  throw ALError(getName(), "GVMsample()", "Couldn't create proxy to itself " +
			  e.toString());
  }

  fTaskMonitor = getParentBroker()->getTaskMonitor();
}



//______________________________________________
// version
//______________________________________________
std::string GVMsample::version() {
  return ALTOOLS_VERSION( GENERICVIDEOMODULE );
}

//______________________________________________
// destructor
//______________________________________________
GVMsample::~GVMsample() {
  // Release Image Header if client code forgot to call unregisterFromVim.
  if (fIplImageHeader)
    cvReleaseImageHeader(&fIplImageHeader);
}

/**
 * registerToVIM
 */
void GVMsample::registerToVIM(const int &pResolution, const int &pColorSpace, const int &pFps) {

  // If we've already registered a module, we need to unregister it first !
  if (fRegisteredToVim) {
    throw ALError(getName(), "registerToVIM()", "A video module has already been "
      "registered. Call unRegisterFromVIM() before trying to register a new module.");
  }

  // GVM Name that we're going to use to register.
  const std::string kOriginalName = "sample_GVM";
  int imgWidth = 0;
  int imgHeight = 0;  
  int imgNbLayers = 0;
  const int kImgDepth = 8;
  fFPS = pFps;

  // Release Image Header if it has been allocated before.
  if (fIplImageHeader)
    cvReleaseImageHeader(&fIplImageHeader);

  getSizeFromResolution(pResolution, imgWidth, imgHeight);
  imgNbLayers = getNumLayersInColorSpace(pColorSpace);

  if (imgWidth == -1 || imgWidth == -1 || imgNbLayers == -1) {
    throw ALError(getName(), "registerToVIM()", "Invalid resolution or color space.");
  }

  // Allocate our Image header.
  fIplImageHeader = cvCreateImageHeader(cvSize(imgWidth, imgHeight), kImgDepth,
    imgNbLayers);

  if (!fIplImageHeader) {
    throw ALError(getName(), "registerToVIM()", "Fail to allocate OpenCv image header.");
  }

  // Call the "subscribe" function with the given parameters.
  fGvmName = fCamProxy->call<std::string>("subscribe", kOriginalName,
               pResolution, pColorSpace, fFPS );

  fLogProxy->info(getName(), " module registered as " + fGvmName);
  
  // Registration is successful, set fRegisteredToVim to true.
  fRegisteredToVim = true;
}


/**
 * unRegisterFromVIM
 */
void GVMsample::unRegisterFromVIM() {

  if (!fRegisteredToVim) {
    return;
	//throw ALError(getName(), "unRegisterFromVIM()", "No video module is currently "
    //  "registered! Call registerToVIM first.");
  }

  // Release Image Header if it has been allocated.
  if (fIplImageHeader)
    cvReleaseImageHeader(&fIplImageHeader);

  fLogProxy->info(getName(), "try to unregister " + fGvmName + " module." );
  fCamProxy->callVoid("unsubscribe", fGvmName);
  fLogProxy->info(getName(), "Done.");

  // UnRegistration is successful, set fRegisteredToVim to false.
  fRegisteredToVim = false;
}



void GVMsample::startGrabbing()
{
	if (!fRegisteredToVim) {
		throw ALError(getName(), "unRegisterFromVIM()", "No video module is currently "
	      "registered! Call registerToVIM first.");
	}

	if(isRunning(fGrabberID)){
		fLogProxy->info(getName(), "Grabber already running. Not starting it again.");
		return;
	}

	fLogProxy->info(getName(), "Testing calling with pcall");
	fGrabberID = fThisProxy->pCall("grabFrames");
}

void GVMsample::stopGrabbing()
{
	if (!fRegisteredToVim) {
		throw ALError(getName(), "unRegisterFromVIM()", "No video module is currently "
	      "registered! Call registerToVIM first.");
	}

	if(isRunning(fGrabberID))
		this->functionStop(fGrabberID);

}


void GVMsample::grabFrames()
{
	//delay between frames, in milliseconds
	int delay = 1000 / fFPS;
	int myID = getMethodID();

	ALCriticalSection section(&fGrabberMutex);

	fLogProxy->info(getName(), "Testing calling");

	while(!fTaskMonitor->isRequireStop(myID)){
		ALImage* imageIn = NULL;

		// Now you can get the pointer to the video structure.
		imageIn = (ALImage*) (fCamProxy->call<int>("getImageLocal", fGvmName));

		if (!imageIn) {
			throw ALError(getName(), "frameGrabber", "Invalid image returned.");
		}

		ALValue image (imageIn->getFrame(), imageIn->getSize());

//		ALValue tst ("Testing image size " + imageIn->getSize());
//		fLogProxy->info(getName(), tst);

		fMemoryProxy->callVoid ("insertData", frameMemoryAddress , image, 0);

		// Now that you're done with the (local) image, you have to release it from the V.I.M.
		fCamProxy->call<int>("releaseImage", fGvmName);

		SleepMs(delay);
	}
}

/**
 * saveImage : save the last image received.
 * @param pName name of the file
 */
void GVMsample::saveImageLocal(const std::string& pName, const std::string& pImageFormat) {

  // Check that a video module has been registered.
  if (!fRegisteredToVim) {
    throw ALError(getName(), "saveImageLocal()",  "No video module is currently "
      "registered! Call registerToVIM() first.");
  }

#ifdef GENERICVIDEOMODULE_IS_REMOTE_ON
  // If this module is running in remote mode, we shouldn't use saveImageLocal.
  throw ALError(getName(), "saveImageLocal()", "Module is run in remote mode, "
    "use saveImageRemote instead !");
#else

  ALImage* imageIn = NULL;

  // Now you can get the pointer to the video structure.
  imageIn = (ALImage*) (fCamProxy->call<int>("getImageLocal", fGvmName));

  if (!imageIn) {
    throw ALError(getName(), "saveImageLocal", "Invalid image returned.");
  }
  
  fLogProxy->info(getName(), imageIn->toString());

  // You can get some image information that you may find useful.
  const int width = imageIn->fWidth;
  const int height = imageIn->fHeight;
  const int nbLayers = imageIn->fNbLayers;
  const int colorSpace = imageIn->fColorSpace;
  const long long timeStamp = imageIn->fTimeStamp;
  const int seconds = (int)(timeStamp/1000000LL);


  // Set the buffer we received to our IplImage header.
  fIplImageHeader->imageData = (char*)imageIn->getFrame();

  ALValue image ((void *)fIplImageHeader->imageData, imageIn->getSize());
  //saveIplImage(fIplImageHeader, pName, pImageFormat, seconds);

  fMemoryProxy->callVoid ("insertData", frameMemoryAddress , image, 0);

  // Now that you're done with the (local) image, you have to release it from the V.I.M.
  fCamProxy->call<int>("releaseImage", fGvmName);
  
#endif
}

 
/**
 * saveImageRemote : save the last image received. To be used if genericvideomodule is a remote module.
 * @param pName name of the file
 */
void GVMsample::saveImageRemote(const std::string& pName, const std::string& pImageFormat ) {

  // Check that a video module has been registered.
  if (!fRegisteredToVim) {
    throw ALError(getName(), "saveImageRemote()",  "No video module is currently "
      "registered! Call registerToVIM() first.");
  }

  ALValue results;

  results = (fCamProxy->call<ALValue>( "getImageRemote", fGvmName ));

  if (results.getType()!= ALValue::TypeArray && results.getSize() != 7) {
    throw ALError(getName(), "saveImageRemote", "Invalid image returned.");
  }

  const int size = results[6].getSize();

  // You can get some image information that you may find useful.
  const int width = (int) results[0];
  const int height = (int) results[1];
  const int nbLayers = (int) results[2];
  const int colorSpace = (int) results[3];
  const long long timeStamp = ((long long)(int)results[4])*1000000LL + ((long long)(int)results[5]);
  const int seconds = (int)(timeStamp/1000000LL);

  // Set the buffer we received to our IplImage header.
  fIplImageHeader->imageData = (char*)(results[6].GetBinary());

  saveIplImage(fIplImageHeader, pName, pImageFormat, seconds); 
}


// Actually perform the cvSaveImage operation.
void GVMsample::saveIplImage(const IplImage* img,
                             const std::string& pName,
                             const std::string& pImageFormat,
                             int seconds) {

  const std::string kImageNameFull = pName + DecToString(seconds) + "." + pImageFormat;

  try { 
    cvSaveImage(kImageNameFull.c_str(), fIplImageHeader);
    fLogProxy->info(getName(), "Image saved as " + kImageNameFull);
  }
  catch(...) {
  	throw ALError(getName(), "saveIplImage()", "OpenCV can't save the image with "
      "this format.");
  }
}

} // namespace AL
